#include "Mat4.hpp"
#include "GMathException.hpp"
#include <string>
#include <iostream>
#include <sstream>
#include <cstring>
#include <cmath>

using std::string;
using std::ostream;
using std::ostringstream;
using std::memset;
using std::sqrt;
using std::pow;
using std::exception;

namespace gmath
{
  template<class T, short S> Mat4<T, S>::Mat4()
  {
    memset(m_, 0, S * S);
  }

  template<class T, short S> Mat4<T, S>::Mat4(T diagonal)
  {
    memset(m_, 0, S * S);    
    this->diagonal(diagonal); 
  }

  template<class T, short S> Mat4<T, S>::Mat4(T* m)
  {
    for (short idx = 0; idx < S * S; idx++)
      m_[idx] = m[idx];
  }

  template<class T, short S> void Mat4<T, S>::identity()
  {
    diagonal(1);
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::mul(Mat4<T, S>& m2)
  {
    Mat4<T, S> result;
    for (short col_idx = 0; col_idx < S; col_idx++)
    {
      for (short row_idx = 0; row_idx < S; row_idx++)
      {
        result.v_[col_idx][row_idx] = v_[0][row_idx] * m2.v_[col_idx][0] +
            v_[1][row_idx] * m2.v_[col_idx][1] + 
            v_[2][row_idx] * m2.v_[col_idx][2];
      }
    }
    return result;
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::mul(T a)
  {
    Mat4<T, S> result;    
    for (short idx = 0; idx < S * S; idx++)
      result.m_[idx] = m_[idx] * a;
    return result;
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::transpose()
  {
    Mat4<T, S> result;
    for (short col_idx = 0; col_idx < S; col_idx++)
      for (short row_idx = 0; row_idx < S; row_idx++)
        result.v_[col_idx][row_idx] = v_[row_idx][col_idx]; 
    return result;
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::inverse()
  {
    T det = this->determinant(); 
    if (det == 0)
      throw GMathException("Cannot inverse matrix.");
    Mat4<T, S> m = matrixOfMinors();
    m = m.matrixOfCofactors();
    m = m.transpose();
    return m.mul(1 / det); 
  }

  template<class T, short S> T Mat4<T, S>::determinant()
  {
    return determinant(allocateMatrix(), S);
  }

  template<class T, short S> T Mat4<T, S>::determinant(T** m, 
      short col_count)
  {
    T det = 0;
    if (col_count == 2)
      det = m[0][0] * m[1][1] - m[1][0] * m[0][1]; 
    else
    {
      for (short col_idx = 0; col_idx < col_count; col_idx++)
      {
        T sign = pow(-1, col_idx + 2); 
        T** minor = this->minorM(m, col_count, col_idx, 0);
        det += sign * m[col_idx][0] * determinant(minor, col_count - 1);
      }
    }
    return det;
  }

  template<class T, short S> T** Mat4<T, S>::minorM(T** m, 
      short col_count, short col_idx, short row_idx)
  {
    T** minor = new T*[col_count - 1];
    short minor_col_idx = 0, minor_row_idx = 0;
    for (short c = 0; c < col_count; c++)
    {
      if (c == col_idx) continue;
      minor[minor_col_idx] = new T[col_count - 1];
      minor_row_idx = 0;
      for (short r = 0; r < col_count; r++)
      {
        if (r == row_idx) continue;
        minor[minor_col_idx][minor_row_idx] = m[c][r];
        minor_row_idx++;
      }
      minor_col_idx++;
    }
    return minor;
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::matrixOfMinors()
  {
    Mat4<T, S> m;
    for (short c = 0; c < S; c++)
    {
      for (short r = 0; r < S; r++)
      {
        T** minor = this->minorM(allocateMatrix(), S, c, r);
        T minor_det = this->determinant(minor, S - 1);
        m.v_[c][r] = minor_det;
      }
    }
    return m;
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::matrixOfCofactors()
  {
    Mat4<T, S> m;
    for (short c = 0; c < S; c++)
      for (short r = 0; r < S; r++)
        m.v_[c][r] = pow(-1, c + r) * v_[c][r];
    return m;
  }

  template<class T, short S> T** Mat4<T, S>::allocateMatrix()
  {
    T** m = new T*[S];
    for (short col_idx = 0; col_idx < S; col_idx++)
    {
      m[col_idx] = new T[S];
      for (short row_idx = 0; row_idx < S; row_idx++)
        m[col_idx][row_idx] = v_[col_idx][row_idx];
    }
    return m;
  }

  template<class T, short S> void Mat4<T, S>::diagonal(T value)
  {
    for (int idx = 0; idx < S * S; idx += S + 1)
      m_[idx] = value;
  }

  template<class T, short S> std::string Mat4<T, S>::toString()
  {
    std::ostringstream ss;
    ss << "{\n";
    for (short col_idx = 0; col_idx < S; col_idx++)
    {
      ss << "  ";
      for (short row_idx = 0; row_idx < S; row_idx++)
      {
        ss << v_[col_idx][row_idx];
        ss << (row_idx != S - 1 ? ", " : "");
      }
      ss << "\n";
    }
    ss << "}";
    return ss.str();
  }

  template<class T, short S> std::string Mat4<T, S>::toString(T** m,
      short col_count)
  {
    std::ostringstream ss;
    ss << "{\n";
    for (short col_idx = 0; col_idx < col_count; col_idx++)
    {
      ss << "  ";
      for (short row_idx = 0; row_idx < col_count; row_idx++)
      {
        ss << m[col_idx][row_idx];
        ss << (row_idx != col_count - 1 ? ", " : "");
      }
      ss << "\n";
    }
    ss << "}";
    return ss.str();
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::operator*(T a)
  {
    return this->mul(a);
  }

  template<class T, short S> Mat4<T, S> Mat4<T, S>::operator*(
      Mat4<T, S>& m2)
  {
    return this->mul(m2);
  }

  template<typename T, short S> ostream& operator<<(
      ostream& out, Mat4<T, S> m)
  {
    out << m.toString();
    return out;
  }

  template ostream& operator<<(ostream& out, Mat4<float, 4> m);
  template ostream& operator<<(ostream& out, Mat4<double, 4> m);
  template class Mat4<float, 4>;
  template class Mat4<double, 4>;
}
